/**
 * @file kalman.cpp
 * @author LinYun (1828951982@qq.com)
 * @brief kalman filter source file
 * @version 0.1
 * @date 2024-08-02
 * 
 * @copyright Copyright (c) 2024
 * 
 */
#include"kalman.h"

using namespace kf;

KalmanFilter::KalmanFilter(int stateNum, int measurementNum,  int controlNum, float T)
{
    this->_stateNum = stateNum;
    this->_measureNum = measurementNum;
    this->_controlNum = controlNum;
    this->_T = T;

    _transitionMat = MatrixXd::Zero(_stateNum,_stateNum);
    _measureMat = MatrixXd::Zero(_measureNum,_stateNum);
    _controlMat = MatrixXd::Zero(_stateNum,_controlNum);
    _postCovMat = MatrixXd::Identity(_stateNum,_stateNum);
    _proNoiseCovMat = MatrixXd::Zero(_stateNum,_stateNum);
    _meaNoiseCovMat = MatrixXd::Zero(_measureNum,_measureNum);


    _stateVec = VectorXd::Zero(_stateNum);
    _controlVec = VectorXd::Zero(_controlNum);
    _processNoise = VectorXd::Zero(_stateNum);
    _measureNoise = VectorXd::Zero(_measureNum);
}

KalmanFilter::~KalmanFilter()
{
}

MatrixXd KalmanFilter::predict(){
    if(_controlNum==0){
        this->_stateVec = this->_transitionMat*this->_stateVec;
    }else{
        this->_stateVec = this->_transitionMat*this->_stateVec + this->_controlMat*this->_controlVec;
    }
    this->_postCovMat = this->_transitionMat*this->_postCovMat*this->_transitionMat.transpose() + this->_proNoiseCovMat;
    return this->_stateVec;
}

MatrixXd KalmanFilter::correct(const VectorXd& measureVec){
    if(measureVec.size()!= _measureNum){
        std::cerr<<"Wrong with dimension of measurement vector!"<<std::endl;
        return {};
    }
    //计算kalman增益, n*p的矩阵
    MatrixXd kt = this->_postCovMat*this->_measureMat.transpose()\
    *(this->_measureMat*this->_postCovMat*_measureMat.transpose()+this->_meaNoiseCovMat).inverse();
    //计算最优估计
    _stateVec = _stateVec+kt*(measureVec - _measureMat*_stateVec);
    //更新后验状态估计协方差
    _postCovMat = (MatrixXd::Identity(_stateNum,_stateNum)-kt*_measureMat)*_postCovMat;
    return _stateVec;
}

bool KalmanFilter::setTansMat(const MatrixXd& transitionMat){
    _transitionMat =transitionMat;
    return true;
}
bool KalmanFilter::setMeasMat(const MatrixXd& measurementnMat){
    _measureMat =measurementnMat;
    return true;
}
bool KalmanFilter::setQ(const MatrixXd& proNoiseMat){
    _proNoiseCovMat = proNoiseMat;
    return true;
}
bool KalmanFilter::setR(const MatrixXd& meaNoiseCovMat){
    _meaNoiseCovMat = meaNoiseCovMat;
    return true;
}

void KalmanFilter::info(){
    std::cout<<" ddddd"<<std::endl;
    std::cout<<MatrixXd::Identity(_stateNum,_stateNum)<<std::endl;

    std::cout<<" ddddd"<<std::endl;
}
